import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import time_processor as tp
import geo_processor as gp

def read_ins_data(ins_path):

    result = []

    num = 0
    with open(ins_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(int(items[0])) # timestamp
            result1.append(float(items[1])) # lat
            result1.append(float(items[2])) # lon
            result1.append(float(items[3])) # hei
            result1.append(float(items[4])) # Roll
            result1.append(float(items[5])) # Pitch
            result1.append(float(items[6])) # Yaw

            result.append(result1)

    return np.array(result)

def show_ins_data(data):
    ax = plt.subplot(111)
    gps_org = [30.595592610010, 114.238722681970,23.715600000000]
    enu_new = gp.geodetic_to_enu_series(data[:,1], data[:,2], data[:,3], gps_org[0], gps_org[1], gps_org[2])
    ax.scatter(enu_new[:,0], enu_new[:,1], c='y', s=3)
    # ax.plot(data[:,1],data[:,2],'r')
    plt.show()

if __name__ == "__main__":
    path = r"/shared/20210902/data/data/"
    ins_path = path + r"ins.txt"
    ins_result = read_ins_data(ins_path)
    show_ins_data(ins_result)
